/**
 * @file Integrator.h
 * @author  
 * @brief 提供划穿式积分器和累加积分器
 * @version 1.0
 * @date 2020-07-27
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include <queue>
#include "lib/Time.h"
#include "modules/AssistModule/LogModule.h"

namespace behavior_controller
{
    template <class DataType>
    class Integrator
    {
    public:
        /**
         * @brief 积分器构造函数
         * @param range 划穿区间 //小于等于0为累加积分，大于0为划穿积分
         * @param interval 积分间隔 //小于等于0时按程序调用的时间来积分，大于0时按照指定的时间间隔来积分
         * @return 
         */
        Integrator(const double &range, const double &interval)
            : range_(range), interval_(interval), value_(0){};

        /**
         * @brief 积分操作
         * @param 要积入的新值
         * @return 
         */
        void Integrate(const DataType data);

        /**
         * @brief 获取队列中每项数值的字符串
         * @param 
         * @return 积分的数值
         */
        std::string GetBufStr();

        /**
         * @brief 获取积分值
         * @param 
         * @return 积分的数值
         */
        double Value();

        /**
         * @brief 积分器复位操作
         * @param 
         * @return
         */
        void Reset();

    private:
        double range_;    //划穿区间
        double interval_;  //积分间隔
        double value_;     //积分值
        double time_diff_; //时间差
        double time_last_ = .0f;

        std::queue<std::pair<double, DataType>> result_buf_; //存储单个 时间×Data 后的值,及当前时间
        std::queue<std::pair<double, DataType>> data_buf_;   //存储相乘之前的值，及时间差值
    };

    /**
     * @brief 积分操作
     * 
     * @tparam DataType 
     * @param data 
     */
    template <class DataType>
    void Integrator<DataType>::Integrate(const DataType data)
    {
        double time_now = Time::Now();
        if (range_ > 0)
        {
            std::pair<double, DataType> result_value;
            std::pair<double, DataType> data_value;

            while ((time_now - result_buf_.front().first > range_) && !result_buf_.empty())
            {
                value_ -= result_buf_.front().second;
                result_buf_.pop();
                data_buf_.pop();
            }
            if (time_last_ > 1e-4)
            {
                if (interval_ > 0)
                    data_value.first = interval_;
                else
                {

                    data_value.first = time_now - time_last_;
                }
                result_value.first = time_now;
                result_value.second = (double)data * data_value.first;
                data_value.second = (double)data;
                value_ += result_value.second;
                result_buf_.emplace(result_value);
                data_buf_.emplace(data_value);
            }
        }
        else
        {
            if (interval_ > 0)
            {
                value_ += data * interval_;
            }
            else
            {
                if (time_last_ > 1e-4)
                {
                    double time_diff = time_now - time_last_;
                    value_ += data * time_diff;
                }
            }
        }
        time_last_ = time_now;
    }

    /**
     * @brief 获取积分器中字符串操作
     * 
     * @tparam DataType 
     * @return std::string 
     */
    template <class DataType>
    std::string Integrator<DataType>::GetBufStr()
    {
        std::stringstream data_stream;
        auto result_queue = result_buf_;
        auto data_queue = data_buf_;
        while (!result_queue.empty() && !data_queue.empty())
        {
            const auto result = result_queue.front();
            const auto data_value = data_queue.front();
            result_queue.pop();
            data_queue.pop();
            data_stream << double(result.second) << " " << double(data_value.first) << " " << double(data_value.second) << " " << result_buf_.size() << std::endl;
        }
        data_stream << std::endl;
        return data_stream.str();
    };

    /**
     * @brief 获取积分值操作
     * 
     * @tparam DataType 
     * @return double 
     */
    template <class DataType>
    double Integrator<DataType>::Value() { return value_; };

    /**
     * @brief 积分器中数据清空
     * 
     * @tparam DataType 
     */
    template <class DataType>
    void Integrator<DataType>::Reset() { value_ = 0; };
} // namespace behavior_controller
